using System;
using Sce.Pss.Core;
using Sce.Pss.Core.Graphics;

namespace UTFramework
{
	public class Frustum
	{
		#region Consts
		
		private const int RIGHT = 0;
		private const int LEFT = 1;
		private const int BOTTOM = 2;
		private const int TOP = 3;
		private const int BACK = 4;
		private const int FRONT = 5;
		private const int A = 0;
		private const int B = 1;
		private const int C = 2;
		private const int D = 3;

		#endregion
		
		#region Variables
		
		private Camera _camera;
		private GraphicsContext _graphics;
		private float[,] _frustrum = new float[6, 4];
		
		#endregion
		public Frustum (Camera camera, GraphicsContext graphics)
		{
			_camera = camera;
			_graphics = graphics;
		}
		
		#region Methods
		
		public void Calculate ()
		{
			Matrix4 proj;           // Матрица проекции
			Matrix4 modl;           // Матрица моделей
			Matrix4 clip;           // Плоскости обрезания

			// glGetFloatv() используется, чтобы извлечь информацию о нашем OpenGL мире.
			// мы передаём GL_PROJECTION_MATRIX чтобы получить информацию о матрице проекций.
			// Функция сохранит информацию в массиве [16].
			proj = Matrix4.Perspective (FMath.Radians (45.0f + _camera.getZoom ()), _graphics.Screen.AspectRatio, 1.0f, 250.0f);
   			

			// Передавая GL_MODELVIEW_MATRIX, мы запрашиваем информацию о матрице моделей.
			// Она также сохранится в массиве [16].
			modl = Matrix4.LookAt (new Vector3 (_camera.getPosX (), _camera.getPosY (), _camera.getPosZ ()), 
                                       new Vector3 (_camera.getViewX (), _camera.getViewY (), _camera.getViewZ ()),
                                       new Vector3 (_camera.getUpX (), _camera.getUpY (), _camera.getUpZ ()));

			// Теперь, имея матрицы проекции и моделей, если мы их скомбинируем, то получим плоскости
			// отсечения. Для этого мы умножим матрицы друг на друга.
			
			clip = Matrix4.Multiply (modl, proj);

			/*clip.M11 = modl.M11 * proj.M11 + modl.M12 * proj.M21 + modl.M13 * proj.M31 + modl.M14 * proj.M41;
			clip.M12 = modl.M11 * proj.M12 + modl.M12 * proj.M22 + modl.M13 * proj.M32 + modl.M14 * proj.M42;
			clip.M13 = modl.M11 * proj.M13 + modl.M12 * proj.M23 + modl.M13 * proj.M33 + modl.M14 * proj.M43;
			clip.M14 = modl.M11 * proj.M14 + modl.M12 * proj.M24 + modl.M13 * proj.M34 + modl.M14 * proj.M44;

			clip.M21 = modl.M21 * proj.M11 + modl.M22 * proj.M21 + modl.M23 * proj.M31 + modl.M24 * proj.M41;
			clip.M22 = modl.M21 * proj.M12 + modl.M22 * proj.M22 + modl.M23 * proj.M32 + modl.M24 * proj.M42;
			clip.M23 = modl.M21 * proj.M13 + modl.M22 * proj.M23 + modl.M23 * proj.M33 + modl.M24 * proj.M43;
			clip.M24 = modl.M21 * proj.M14 + modl.M22 * proj.M24 + modl.M23 * proj.M34 + modl.M24 * proj.M44;

			clip.M31 = modl.M31 * proj.M11 + modl.M32 * proj.M21 + modl.M33 * proj.M31 + modl.M34 * proj.M41;
			clip.M32 = modl.M31 * proj.M12 + modl.M32 * proj.M22 + modl.M33 * proj.M32 + modl.M34 * proj.M42;
			clip.M33 = modl.M31 * proj.M13 + modl.M32 * proj.M23 + modl.M33 * proj.M33 + modl.M34 * proj.M43;
			clip.M34 = modl.M31 * proj.M14 + modl.M32 * proj.M24 + modl.M33 * proj.M34 + modl.M34 * proj.M44;

			clip.M41 = modl.M41 * proj.M11 + modl.M42 * proj.M21 + modl.M43 * proj.M31 + modl.M44 * proj.M41;
			clip.M42 = modl.M41 * proj.M12 + modl.M42 * proj.M22 + modl.M43 * proj.M32 + modl.M44 * proj.M42;
			clip.M43 = modl.M41 * proj.M13 + modl.M42 * proj.M23 + modl.M43 * proj.M33 + modl.M44 * proj.M43;
			clip.M44 = modl.M41 * proj.M14 + modl.M42 * proj.M24 + modl.M43 * proj.M34 + modl.M44 * proj.M44;*/

			// Теперь нам нужно получить стороны frustum'а. Чтобы сделать это,
			// возьмём обрезающие плоскости, полученные выше, и из них получим стороны.

			// получаем ПРАВУЮ сторону frustum'а
			_frustrum [RIGHT, A] = clip.M14 - clip.M11;
			_frustrum [RIGHT, B] = clip.M24 - clip.M21;
			_frustrum [RIGHT, C] = clip.M34 - clip.M31;
			_frustrum [RIGHT, D] = clip.M44 - clip.M41;

			// Теперь, имея нормаль (A,B,C) и расстояние (D) к плоскости,
			// нам нужно нормализовать нормаль и дистанцию						

			// Нормализуем правую сторону
			NormalizePlane (ref _frustrum, RIGHT);

			// получаем ЛЕВУЮ сторону frustum'а
			_frustrum [LEFT, A] = clip.M14 + clip.M11;
			_frustrum [LEFT, B] = clip.M24 + clip.M21;
			_frustrum [LEFT, C] = clip.M34 + clip.M31;
			_frustrum [LEFT, D] = clip.M44 + clip.M41;

			// Нормализуем ЛЕВУЮ сторону
			NormalizePlane (ref _frustrum, LEFT);

			// получаем нижнюю сторону frustum'а
			_frustrum [BOTTOM, A] = clip.M14 + clip.M12;
			_frustrum [BOTTOM, B] = clip.M24 + clip.M22;
			_frustrum [BOTTOM, C] = clip.M34 + clip.M32;
			_frustrum [BOTTOM, D] = clip.M44 + clip.M42;

			// Нормализуем нижнюю сторону
			NormalizePlane (ref _frustrum, BOTTOM);

			// получаем верхнюю сторону frustum'а
			_frustrum [TOP, A] = clip.M14 - clip.M12;
			_frustrum [TOP, B] = clip.M24 - clip.M22;
			_frustrum [TOP, C] = clip.M34 - clip.M32;
			_frustrum [TOP, D] = clip.M44 - clip.M42;

			// Нормализуем верхнюю сторону
			NormalizePlane (ref _frustrum, TOP);

			// получаем заднюю сторону frustum'а
			_frustrum [BACK, A] = clip.M14 - clip.M13;
			_frustrum [BACK, B] = clip.M24 - clip.M23;
			_frustrum [BACK, C] = clip.M34 - clip.M33;
			_frustrum [BACK, D] = clip.M44 - clip.M43;

			// Нормализуем заднююсторону
			NormalizePlane (ref _frustrum, BACK);

			// получаем переднюю сторону frustum'а
			_frustrum [FRONT, A] = clip.M14 + clip.M13;
			_frustrum [FRONT, B] = clip.M24 + clip.M23;
			_frustrum [FRONT, C] = clip.M34 + clip.M33;
			_frustrum [FRONT, D] = clip.M44 + clip.M43;

			// Нормализуем переднюю сторону
			NormalizePlane (ref _frustrum, FRONT);
		}
		
		bool SphereInFrustum (float x, float y, float z, float radius)
		{
    
			// учесть ещё и радиус вокруг точки. Например, одно то, что центр сферы находится вне 
			// пирамиды, ещё не значит, что и все её точки находятся снаружи. Поэтому вместо проверки,
			// меньше ли результат формулы нуля (<=0), нужно прибавить к нулю отрицательный радиус сферы.

			// Проходим через все стороны пирамиды
			for (int i = 0; i < 6; i++) {
				// Если центр сферы дальше от плоскости, чем её радиус
				if (_frustrum [i, A] * x + _frustrum [i, B] * y + _frustrum [i, C] * z + 
                _frustrum [i, D] <= -radius) {
					// То и вся сфера снаружи, возвращаем false
					return false;
				}
			}

			// Иначе сфера внутри
			return true;
		}
		
		#endregion
		
		#region Private Methods
		
		private void NormalizePlane (ref float[,] frustum, int side)
		{
			// Вычисляем величину нормали плоскости (точку A B C)
			// Помните, что (A, B, C) плоскости - то же самое, что (X, Y, Z) для нормали.
			// Чтобы вычислить величину, используем формулу: sqrt(x^2 + y^2 + z^2)
			float magnitude = (float)Math.Sqrt (frustum [side, A] * frustum [side, A] +
                                   frustum [side, B] * frustum [side, B] +
                                   frustum [side, C] * frustum [side, C]);

			// Затем делим значения плоскости на её величину.
			// После этого с плоскостью будет легче работать.
			frustum [side, A] /= magnitude;
			frustum [side, B] /= magnitude;
			frustum [side, C] /= magnitude;
			frustum [side, D] /= magnitude;
		}
		
		#endregion
		
		
	}
}

